/*
 * webcam.cpp
 *
 *  Created on: 18-11-2011
 *      Author: koki
 *      Author: Tomek Lipert
 */

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <iostream>
#include <string>
#include <ctime>
#include <cmath>
#include <cstdlib>
#include <sys/time.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#define OBJB1 0.1665
#define OBJB2 0.0004
#define OBJB3 0.0001

using namespace std;
int IP_desc;

char* bufor = new char[30];

void connect() {
	//char buffer[128]="abcdefghi\0";
	IP_desc = socket(AF_INET, SOCK_STREAM, 0);
//cout << "aaa " << IP_desc;
	if (IP_desc < 0) {

		printf("\n\n  Blad deskryptora\n\n");

	}

	printf("\n IP desc = %d\n\n", IP_desc);

	struct in_addr is;

	is.s_addr = INADDR_ANY;

	struct sockaddr_in local;

	local.sin_family = AF_INET;
	local.sin_port = htons(2000);
	local.sin_addr = is;

	//int server_ID = bind( IP_desc,(struct sockaddr*) &local ,sizeof(struct sockaddr));

	//printf("\nStatus serwera %d\n", server_ID);

	struct in_addr ia;

	ia.s_addr = inet_addr("127.0.0.1");

	struct sockaddr_in dest;

	dest.sin_family = AF_INET;
	dest.sin_port = htons(1234);
	dest.sin_addr = ia;

	int connect_ID = connect(IP_desc, (struct sockaddr*) &dest,
			sizeof(struct sockaddr));

	printf("\nStatus połaczenia %d\n", connect_ID);

	//write(IP_desc, buffer, 128);
}

void sendColor() {

	bufor[0] = 'C';
	bufor[1] = ' ';
	bufor[2] = '1';
	bufor[3] = '0';
	bufor[4] = '0';
	bufor[5] = ' ';
	bufor[6] = '0';
	bufor[7] = ' ';
	bufor[8] = '0';
	bufor[9] = '\n';
	//bufor[10] = '\0';


	write(IP_desc, bufor, 10);
}

void sendAngle(float angle) {
//angle = 0.75;
	char* str = new char[5];
	for (int i = 0; i < 9; i++){
			bufor[i] ='0';
		}

	sprintf(str, "%.4g", angle);
	//cout << str << endl;
	bufor[0] = 'A';
	bufor[1] = ' ';
	bufor[2] = str[0];
	bufor[3] = str[1];
	bufor[4] = str[2];
	bufor[5] = str[3];
	bufor[6] = str[4];
	bufor[7] = '\n';
	//bufor[8] = '\0';

	for (int i = 0; i < 9; i++){
		if (bufor[i] == ',') bufor[i] = '.';
	}

	//cout << "Bufor: " <<bufor[0];

	write(IP_desc, bufor, 8);
}

IplImage * ContrastBrightness(IplImage *src, int Contrast, int Brightness) {

	if (Contrast > 100)
		Contrast = 100;
	if (Contrast < -100)
		Contrast = -100;
	if (Brightness > 100)
		Brightness = 100;
	if (Brightness < -100)
		Brightness = -100;

	uchar lut[256];

	CvMat* lut_mat;
	int hist_size = 256;
	float range_0[] = { 0, 256 };
	float* ranges[] = { range_0 };

	int i;

	IplImage * dest = cvCloneImage(src);

	IplImage * GRAY = cvCreateImage(cvGetSize(src), src->depth, 1);

	if (src->nChannels == 3) {
		cvCvtColor(src, GRAY, CV_RGB2GRAY);
	} else {
		cvCopyImage(src, GRAY);
	}

	lut_mat = cvCreateMatHeader(1, 256, CV_8UC1);
	cvSetData(lut_mat, lut, 0);

	/*
	 * The algorithm is by Werner D. Streidt
	 * (http://visca.com/ffactory/archives/5-99/msg00021.html)
	 */

	if (Contrast > 0)

	{
		double delta = 127. * Contrast / 100;
		double a = 255. / (255. - delta * 2);
		double b = a * (Brightness - delta);
		for (i = 0; i < 256; i++) {
			int v = cvRound(a * i + b);

			if (v < 0)
				v = 0;
			if (v > 255)
				v = 255;
			lut[i] = v;
		}
	} else {
		double delta = -128. * Contrast / 100;

		double a = (256. - delta * 2) / 255.;
		double b = a * Brightness + delta;
		for (i = 0; i < 256; i++) {
			int v = cvRound(a * i + b);
			if (v < 0)
				v = 0;

			if (v > 255)
				v = 255;
			lut[i] = v;
		}
	}

	if (src->nChannels == 3) {

		IplImage * R = cvCreateImage(cvGetSize(src), src->depth, 1);

		IplImage * G = cvCreateImage(cvGetSize(src), src->depth, 1);
		IplImage * B = cvCreateImage(cvGetSize(src), src->depth, 1);

		cvCvtPixToPlane(src, R, G, B, NULL);

		// PERFORM IT ON EVERY CHANNEL
		cvLUT(R, R, lut_mat);

		cvLUT(G, G, lut_mat);
		cvLUT(B, B, lut_mat);

		cvCvtPlaneToPix(R, G, B, NULL, dest);

		cvReleaseImage(&R);
		cvReleaseImage(&G);
		cvReleaseImage(&B);

	} else {

		//PERFORM IT ON THE CHANNEL
		cvLUT(GRAY, dest, lut_mat);
	}

	cvReleaseImage(&GRAY);
	cvReleaseMat(&lut_mat);

	return dest;

}

inline unsigned long getTimeInMSEC() {

	timeval tv;

	//DID_CHECK(gettimeofday(&tv,NULL), 0);

	gettimeofday(&tv, NULL);

	return (static_cast<unsigned long>(tv.tv_sec) * 1000UL) +

	(static_cast<unsigned long>(tv.tv_usec) / 1000UL);

}

int main(int argc, const char* argv[]) {
	connect();
	sendColor();
	//sendAngle(0.123456);

	bool log = 0;

	if (argc > 1) {
		log = 1;
	}

	CvCapture* capture = cvCaptureFromCAM(0);

	cvNamedWindow("afterEffects", CV_WINDOW_AUTOSIZE);

	CvScalar colorB = CV_RGB( 0, 255, 0 );

	int minimalnaWielkosc = 10000, pole = 0, liczbaRamek = 0,
			liczbaRamekRozpoznanych = 0;
	float iloraz = 1;

	unsigned long start = getTimeInMSEC();

	while (1) {

		IplImage* frame = cvQueryFrame(capture);

		frame = ContrastBrightness(frame, 70, 40);

		minimalnaWielkosc = (640 / 12) * (480 / 12);

		IplImage *kopia = cvCreateImage(cvSize(640, 480), IPL_DEPTH_8U, 1);

		cvFlip(frame, frame, 2);

		cvCvtColor(frame, kopia, CV_RGB2GRAY);

		cvSmooth(kopia, kopia, CV_GAUSSIAN, 11, 11, 2, 2);
		cvCanny(kopia, kopia, 10, 60, 3);

		CvMemStorage* storage = cvCreateMemStorage(0);
		CvSeq* contour = 0;
		cvFindContours(kopia, storage, &contour, sizeof(CvContour),
				CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

		bool elem1 = 0;
		int rX1 = 0, rY1 = 0;

		for (int i = 0; contour != 0; contour = contour->h_next, i++) {

			static CvMoments* moments = new CvMoments();
			cvMoments(contour, moments);
			static CvHuMoments* huMoments = new CvHuMoments();
			cvGetHuMoments(moments, huMoments);

			CvRect r = cvBoundingRect(contour, 1);

			iloraz = (float) (abs(r.width - r.height))
					/ (float) max(r.width, r.height);

			pole = r.width * r.height;

			if ((pole > minimalnaWielkosc) && (iloraz < 0.4)
					&& (abs(huMoments->hu1 - OBJB1) < 0.5)
					&& (abs(huMoments->hu2 - OBJB2) < 0.005)) { // kwadrat

				cvDrawContours(frame, contour, colorB, colorB, CV_FILLED);

				if (elem1 == 0) {

					elem1 = 1;
					rX1 = r.x;
					rY1 = r.y;

				}

				if ((elem1 == 1)
						&& ((abs(rY1 - r.y) > 50) || ((abs(rX1 - r.x) > 50)))) {

					double a = ((double) (rY1 - r.y)) / ((double) (rX1 - r.x));
					a = atan(a);
					std::cout << a * 180 / 3.14 << endl;
					//sendColor();
					sendAngle(a*1.3);

					liczbaRamekRozpoznanych++;
					elem1 = 0;

				}

				if (log) {
					std::cout << huMoments->hu1 << " " << huMoments->hu2
							<< std::endl;
				}

			}

			cvReleaseMemStorage(&storage);

		}

		cvShowImage("afterEffects", frame);

		cvReleaseImage(&kopia);
		cvReleaseImage(&frame);

		liczbaRamek++;

		if ((cvWaitKey(10) & 255) == 27) {

			break;

		}

	}

	unsigned long delta = getTimeInMSEC() - start;

	std::cout << "FPS: "
			<< (double) ((double) (liczbaRamek) / (double) (delta) * 1000)
			<< std::endl << "% ROZPOZNANYCH: "
			<< ((liczbaRamekRozpoznanych * 100) / liczbaRamek) << std::endl;

	cvReleaseCapture(&capture);
	cvDestroyWindow("mywindow");
	cvDestroyWindow("afrerEffects");

	return 0;
}

